%% NMPC Trajectory tracking example of diff drive robot in simulation with bounded disturbance
% 2024/03/25
% Author: by Liang Hongpeng, 171186623@qq.com

%% Tips:
% 首先运行工程目录下的acados_env_variables_windows.m, 否则提示找不到相关函数

%% System description
% x' = f(x,u) + Bw * w

%% Initiallization
clear all       % 清空所有变量
close all       % 关闭所有图形窗口
clc             % 清空命令行
import casadi.*

%% Disturbance
Bw = [1,0,0;0,1,0;0,0,3];                       % 扰动输入矩阵

wmax_x = 0.006;                                 % 状态x扰动的最大值0.03m/s 0.021
wmax_y = 0.006;                                 % 状态y扰动的最大值0.03m/s
wmax_theta = 0.006;                             % 状态theta扰动的最大值0.09rad/s
wmax = [wmax_x; wmax_y; wmax_theta];
wmax_norm = norm(wmax);
W_vertex = [wmax_x , wmax_y , wmax_theta;       % 扰动集合的顶点
            wmax_x , -wmax_y, wmax_theta;
            -wmax_x, wmax_y , wmax_theta; 
            -wmax_x, -wmax_y, wmax_theta;
            wmax_x , wmax_y , -wmax_theta; 
            wmax_x , -wmax_y, -wmax_theta;
            -wmax_x, wmax_y , -wmax_theta; 
            -wmax_x, -wmax_y, -wmax_theta;];
% 在这个特定的代码中，W表示一个三维的矩形，表示在xyz轴上的扰动范围
W = Polyhedron(W_vertex);                       % 构造扰动集合的Polyhedron对象

load('./data/test0.006.mat');                   % w

%% 可调参数
N = 35;                                         % 预测步长          bug:10 
T = 21.0;                                       % 预测总时长[s]          6 
dt = T / N;                                     % 控制周期 21/35=0.6

%% 路径规划
% 定义参考轨迹:使用多项式拟合给定的路径点

% trajectory2
% 生成圆的参数
radius  = 2;                                    % 第一个圆的半径
center1 = [-2, 0];                              % 第一个圆的中心在坐标轴左边
center2 = [2, 0];                               % 第二个圆的中心在坐标轴右边

ref_x = [];
ref_y = [];
ref_theta = [];
ref_v = [];

% 第一段半圆
theta1 = linspace(0, pi, 50);                   % 生成圆的角度
adjust = pi;
[x_seq,y_seq,theta_seq] = path_generation(theta1,center1,radius,adjust);
%ref_x = [ref_x,x_seq];ref_y = [ref_y,y_seq];ref_theta = [ref_theta,theta_seq];

% 第二段半圆
theta1 = linspace(pi, 2*pi, 50);                % 生成圆的角度
adjust = 0;
[x_seq,y_seq,theta_seq] = path_generation(theta1,center1,radius,adjust);
%ref_x = [ref_x,x_seq];ref_y = [ref_y,y_seq];ref_theta = [ref_theta,theta_seq];
%x_sup = [ref_x(1:N)];y_sup = [ref_y(1:N)];theta_sup = [ref_theta(1:N)];

% 第三段半圆
theta2 = linspace(pi, 0, 50);                   % 生成圆的角度
adjust = 0;
[x_seq,y_seq,theta_seq] = path_generation(theta2,center2,radius,adjust);
%ref_x = [ref_x,x_seq];ref_y = [ref_y,y_seq];ref_theta = [ref_theta,theta_seq];

% 第四段半圆
theta2 = linspace(0, -pi, 50);                  % 生成圆的角度
adjust = -pi;
[x_seq,y_seq,theta_seq] = path_generation(theta2,center2,radius,adjust);
%ref_x = [ref_x,x_seq];ref_y = [ref_y,y_seq];ref_theta = [ref_theta,theta_seq];

% 第五段半圆
%ref_x = [ref_x,x_sup];
%ref_y = [ref_y,y_sup];
%ref_theta = [ref_theta,theta_sup];

% 偏航角跳变处理
%ref_theta = angleseq_bound(ref_theta);

% 计算参考速度
%ref_v = calc_vel(ref_x, ref_y, dt);

% 参考轨迹图
% figure()
% %'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
% %set(gca,'FontName','Times New Roman','FontSize',14);
% plot(ref_x, ref_y, 'r.', 'LineWidth', 0.8)
% 
% %设置坐标轴名称的字体，可以覆盖上述设置
% xlabel('x (m)','fontsize',16);ylabel('y (m)','fontsize',16);
% %设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
% legend({'ref'},'Orientation','horizontal');
% %设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
% set(legend,'Location','NorthEast');
% 
% axis equal;  % 保持坐标轴比例相等
% grid on;

%% 路径规划(封装为函数接口调用)
[ref_x,ref_y,ref_theta,ref_v] = plane_path_generation(N, dt);

%% 初始化
ns = length(ref_x);
Tf = (ns - N + 1) * dt;                     % 最大仿真时间[s] 10.00
Nsim = round(Tf / dt);                      % 仿真步数
nx = 3;                                     % 状态变量数 3
nu = 2;                                     % 控制输入变量数 2

last_theta_r=pi/2;
x_sim               = [0; 0; pi/2];         %理想2d pose
xc_sim              = x_sim;                %当前2d pose

x_sim_history       = zeros(nx, Nsim+1);    % 存储标称状态
x_sim_history(:,1)  = x_sim; 
xc_sim_history      = zeros(nx, Nsim+1);    % 存储实际状态
xc_sim_history(:,1) = x_sim;  
u_sim_history       = zeros(nu, Nsim);      % 存储标称输入
uc_sim_history      = zeros(nu, Nsim);      % 存储实际输入
ref_w               = zeros(1, Nsim);       % 存储参考角速度输入

u_opt               = zeros(nu,1);
times               = zeros(1, Nsim+1);
tcomp_max = 0.0;                            % 最大求解时间            

w_nomi              = zeros(nx, Nsim+1);    %存储扰动

%% acados setting
ocp_nmpc = acados_nmpc_ocp_get();           % 生成nmpc_ocp求解器对象

% ocp_get: supported values are:
% x, u, z, pi, lam, sl, su, t, sens_x, sens_u, sens_pi, status, sqp_iter, time_tot, time_lin, time_reg, time_qp_sol, stat, qp_solver_cond_H, qp_A, qp_B, qp_Q, qp_R, qp_S, qp_b, qp_q, qp_r

% ocp_set: supported values are:
% p, constr_x0, constr_lbx, constr_ubx, constr_C, constr_D, constr_lg, constr_ug, constr_lh, constr_uh, constr_lbu, constr_ubu, cost_y_ref[_e], sl, su, x, xdot, u, pi, lam, z,  cost_Vu, 
% cost_Vx, cost_Vz, cost_W, cost_Z, cost_Zl, cost_Zu, cost_z, cost_zl, cost_zu, init_x, init_u, init_z, init_xdot, init_gnsf_phi, init_pi, nlp_solver_max_iter, qp_warm_start, warm_start_first_qp, print_level

%% NMPC Optimization problem using acados
for k = 1:Nsim
%for k = 1 : 10
    % 更新参考状态
    j = k+N-1;
    ref = [ref_x(k:j); ref_y(k:j); ref_theta(k:j)];
    v_r = ref_v(k);
    theta_r = ref_theta(k); w_r=angle_bound(theta_r-last_theta_r)/dt; last_theta_r=theta_r;
    u_r = [v_r;ref_theta(k)];

    % 从扰动集合中随机选择扰动
    %w(:,k) = pick_random_Disturbance(W);  

    % 在循环开始处记录时间
    iteration_start_time = tic;

    % 应用TubeNMPC方法控制律
    [uc_opt, u_opt, P_nom, alpha_nom] = TubeNMPC_Controller(ref, xc_sim, x_sim, u_r, k, ocp_nmpc, u_opt); %u_r??

    % 输出耗时信息
    iteration_elapsed_time = toc(iteration_start_time);             % 计算循环耗时
    fprintf('Iteration %d took %.4f seconds\n', k, iteration_elapsed_time);

    % 最大求解时间统计
    if iteration_elapsed_time > tcomp_max
        tcomp_max = iteration_elapsed_time;
    end

    % 将控制量输入对象
    x_sim = sim_robot_model(x_sim, u_opt, dt);                      % 理想系统
    xc_sim = sim_robot_model(xc_sim,uc_opt,dt) + Bw * w(:,k);       % 真实带噪声

    % 保存历史数据
    u_sim_history(:,k) = u_opt;
    x_sim_history(:,k+1) = x_sim;                                   % 理想轨迹记录
    uc_sim_history(:,k) = uc_opt;
    xc_sim_history(:,k+1) = xc_sim;                                 % 实际轨迹记录
    ref_w(k) = w_r;
    times(k+1) = k*dt;

    % 图1：实际轨迹与参考轨迹对比
    figure(1)
    %'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
    %set(gca,'FontName','Times New Roman','FontSize',14);
    plot(ref_x, ref_y, 'r.', 'LineWidth', 0.8)
    hold on
    plot(x_sim_history(1, :), x_sim_history(2, :), 'b.', 'LineWidth', 0.5)
    hold on
    plot(xc_sim_history(1, :), xc_sim_history(2, :), 'g.', 'LineWidth', 0.5)
    hold on

    % 迭代绘制每一步的状态tube范围: 椭圆表示状态偏差的不确定性
    % P : 椭圆的协方差矩阵，表示椭圆的形状和方向。
    % mu * wmax_norm^2/lambda0 : 椭圆的半长轴长度，通过对参数进行线性组合计算得到。这个值决定了椭圆的大小。
    % [xn(1,i), xn(2,i)] : 椭圆的中心位置，即状态偏差的当前值。
    % 'y' : 椭圆的颜色，这里是黄色。
    draw_ellip2(P_nom, alpha_nom, [x_sim(1), x_sim(2)] ,'y')
    hold on

    %设置坐标轴名称的字体，可以覆盖上述设置
    xlabel('x (m)','fontsize',16);ylabel('y (m)','fontsize',16);
    %设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
    legend({'ref','nomi-sim','tube-sim'},'Orientation','horizontal');
    %设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
    set(legend,'Location','NorthEast');
%     %设置x轴范围
%     xlim([-2,2]);
%     %设置x刻度如何显示
%     xticks(-2:1:2);
%     %设置y轴范围
%     ylim([-2,2]);
%     %设置y刻度如何显示
%     yticks(-2:1:2);
    axis equal;  % 保持坐标轴比例相等
    grid on;

end

%% Plots
% 绘制结果
% 图2：x,y,theta状态随时间变化情况
figure;
subplot(3, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times, ref_x(1:length(times)), 'r-', 'LineWidth', 0.8);
hold on;
plot(times, x_sim_history(1, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times, xc_sim_history(1, :), 'g--', 'LineWidth', 0.8);
ylabel('x (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-x','nomi-x','tube-x'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times, ref_y(1:length(times)), 'r-', 'LineWidth', 0.8);
hold on;
plot(times, x_sim_history(2, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times, xc_sim_history(2, :), 'g--', 'LineWidth', 0.8);
ylabel('y (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-y','nomi-y','tube-y'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 3);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times, ref_theta(1:length(times)), 'r-', 'LineWidth', 0.8);
hold on;
plot(times, x_sim_history(3, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times, xc_sim_history(3, :), 'g--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('\theta (rad)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-\theta','nomi-\theta','tube-\theta'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;
%xlim([t(1), t(end)]); % 设置x轴范围

% 图3：v,omega输入随时间变化情况
len = length(u_sim_history);
figure;
subplot(2, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), ref_v(1:len), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:len), u_sim_history(1, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times(1:len), uc_sim_history(1, :), 'g--', 'LineWidth', 0.8);
ylabel('线速度 (m/s)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-v','nomi-v','tube-v'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(2, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), ref_w(1:len), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:len), u_sim_history(2, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times(1:len), uc_sim_history(2, :), 'g--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('角速度 (rad/s)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-\omega','nomi-\omega','tube-\omega'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

% 图4：实际轨迹与参考轨迹之差 nomi-ref
len = length(xc_sim_history);
tube_delta_x = ref_x(1:len) - xc_sim_history(1, :);
tube_delta_y = ref_y(1:len) - xc_sim_history(2, :);
tube_delta_theta = ref_theta(1:len) - xc_sim_history(3, :);
figure;
subplot(3, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times, tube_delta_x(1,:), 'r--', 'LineWidth', 0.8);
ylabel('\deltax (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'\deltax'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times, tube_delta_y(1,:), 'g--', 'LineWidth', 0.8);
ylabel('\deltay (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'\deltay'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 3);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times, tube_delta_theta(1,:), 'b--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('\delta\theta (rad)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'\delta\theta'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

% 图5：噪声
len=len-1;
figure;
subplot(3, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), w(1,1:len), 'r--', 'LineWidth', 0.8);
ylabel('wx (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'wx'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), w(2,1:len), 'g--', 'LineWidth', 0.8);
ylabel('wy (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'wy'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 3);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), w(3,1:len), 'b--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('w\theta (rad)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'w\theta'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

%% go embedded
% to generate templated C code
% ocp_nmpc.generate_c_code;
